#include<stdio.h>
#include<stdlib.h>
#include<string.h>
#include<Windows.h>
#include<psapi.h>
 
static PIMAGE_DOS_HEADER nt=0;
static size_t ntlnkbase=0,ntrealbase=0;
 
unsigned GetServiceIndexFromNtdll(char*name)
{
    static HMODULE ntdll=0;
    if(!ntdll)ntdll=GetModuleHandleW(L"ntdll.dll");
    return *(unsigned*)((char*)GetProcAddress(ntdll,name)+1);
}
 
unsigned GetServiceIndex(char*name)
{
        return *(unsigned*)((char*)GetProcAddress((HMODULE)nt,name)+1);
}
 
size_t GetServiceRVA(char*name)
{
        return (size_t)GetProcAddress((HMODULE)nt,name)-(size_t)nt;
}
 
size_t*GetKiServiceTable(void)
{
        unsigned NtCreateFile_Id,NtOpenFile_Id;size_t NtCreateFile_RVA,NtOpenFile_RVA,*p;MODULEINFO info;
        NtCreateFile_Id=GetServiceIndex("ZwCreateFile"),NtOpenFile_Id=GetServiceIndex("ZwOpenFile");
        NtCreateFile_RVA=GetServiceRVA("NtCreateFile"),NtOpenFile_RVA=GetServiceRVA("NtOpenFile");
        if(GetModuleInformation((HANDLE)-1,(HMODULE)nt,&info,sizeof(MODULEINFO)))
        for(p=(size_t*)nt;(size_t)p<(size_t)nt+info.SizeOfImage;++p)
        {
                if(*p==NtCreateFile_RVA+ntlnkbase&&p[NtOpenFile_Id-NtCreateFile_Id]==NtOpenFile_RVA+ntlnkbase)return p-NtCreateFile_Id;
        }
        return 0;
}
 
void*GetServiceFromTable(size_t*ServiceTable,unsigned id)
{
        return (void*)(ServiceTable[id]-ntlnkbase+ntrealbase);
}
 
size_t GetFileOffset(void*BaseAddress,size_t RVA,PIMAGE_SECTION_HEADER*SectionHeader)
{
    size_t base=(size_t)BaseAddress;PIMAGE_NT_HEADERS ntheaders=(PIMAGE_NT_HEADERS)(base+((PIMAGE_DOS_HEADER)base)->e_lfanew);
    PIMAGE_SECTION_HEADER sectionheader=(PIMAGE_SECTION_HEADER)(ntheaders+1);
    while((size_t)sectionheader<base+ntheaders->OptionalHeader.SizeOfHeaders)
    {
        if(RVA>=sectionheader->VirtualAddress&&RVA<sectionheader->VirtualAddress+sectionheader->SizeOfRawData)
        {
            if(SectionHeader)*SectionHeader=sectionheader;
            return RVA-sectionheader->VirtualAddress+sectionheader->PointerToRawData;
 
        }
        ++sectionheader;
    }
    return 0;
}
 
void*MapImage(wchar_t*FileName)
{
        HANDLE hFile,hSection;void*view;
        hFile=CreateFileW(FileName,FILE_READ_DATA,FILE_SHARE_READ|FILE_SHARE_DELETE,0,OPEN_EXISTING,0,0);
        if(hFile!=INVALID_HANDLE_VALUE)
        {
            hSection=CreateFileMappingW(hFile,0,PAGE_READONLY|SEC_COMMIT,0,0,0);
            CloseHandle(hFile);
            if(hSection)
            {
                view=MapViewOfFileEx(hSection,FILE_MAP_READ,0,0,0,0);
                CloseHandle(hSection);
                return view;
            }
        }
        return 0;
}
 
size_t*GetW32pServiceTable(void*FileView)
{
    unsigned char*base=(unsigned char*)FileView;PIMAGE_NT_HEADERS ntheaders=(PIMAGE_NT_HEADERS)(base+((PIMAGE_DOS_HEADER)base)->e_lfanew);
    PIMAGE_SECTION_HEADER sectionheader;
    unsigned char*entry=base+GetFileOffset(FileView,ntheaders->OptionalHeader.AddressOfEntryPoint,&sectionheader);
    size_t lnkbase=ntheaders->OptionalHeader.ImageBase,i;
    for(i=0;i<sectionheader->SizeOfRawData;++i)
    {
        if(entry[i]==0x68)
        {
            if(entry[i+5]==0xff&&entry[i+6]==0x15)
            {
                size_t calladdr;PIMAGE_THUNK_DATA impthk;PIMAGE_IMPORT_BY_NAME imp;
                memcpy(&calladdr,entry+i+7,4);
                calladdr=GetFileOffset(FileView,calladdr-lnkbase,0);
                if(!calladdr)continue;
                impthk=(PIMAGE_THUNK_DATA)(base+calladdr);
                imp=(PIMAGE_IMPORT_BY_NAME)(base+GetFileOffset(FileView,impthk->u1.AddressOfData,0));
                if((void*)imp==FileView)continue;
                if(memcmp(imp->Name,"KeAddSystemServiceTable",sizeof"KeAddSystemServiceTable"-1)==0)
                {
                    size_t W32pServiceTable;
                    memcpy(&W32pServiceTable,entry+i+1,4);
                    W32pServiceTable=GetFileOffset(FileView,W32pServiceTable-lnkbase,0);
                    if(W32pServiceTable)return (size_t*)(W32pServiceTable+base);
                }
            }
        }
    }
    return 0;
}
 
int main(int argc, char* argv[])
{
        size_t*ServiceTable;DWORD n;wchar_t filename[256];
        EnumDeviceDrivers((void**)&ntrealbase,sizeof ntrealbase,&n);
        GetDeviceDriverBaseNameW((void*)ntrealbase,filename,256);
        nt=(PIMAGE_DOS_HEADER)LoadLibraryExW(filename,0,DONT_RESOLVE_DLL_REFERENCES);
        ntlnkbase=((PIMAGE_NT_HEADERS)((size_t)nt+nt->e_lfanew))->OptionalHeader.ImageBase;
        //ServiceTable=GetKiServiceTable();
        ServiceTable=GetW32pServiceTable(MapImage(L"C:\\windows\\system32\\win32k.sys"));
        printf("%x",ServiceTable);
        getchar();
        //printf("%p",MapImage(L"C:\\windows\\system32\\win32k.sys"));
        //printf("table RVA:%x\n",(size_t)ServiceTable-(size_t)nt);
        //printf("address of NtAllocateVirtualMemory:%p\n",GetServiceFromTable(ServiceTable,GetServiceIndexFromNtdll("NtAllocateVirtualMemory")));
        return 0;
}